<xsl:stylesheet version="2.0"
  xmlns:xsl="http://www.w3.org/1999/XSL/Transform"
  xmlns="http://schemas.datacontract.org/2004/07/Microsoft.Robotics.Runtime"
  xmlns:r="http://schemas.datacontract.org/2004/07/Microsoft.Robotics.Runtime"
  xmlns:k="http://schemas.datacontract.org/2004/07/Microsoft.Robotics.Agents.Core.Kinect"
  xmlns:l="http://schemas.datacontract.org/2004/07/Microsoft.Robotics.Navigation.Runtime.Localization"
  xmlns:m="http://schemas.datacontract.org/2004/07/Test.Robotics.Navigation"
  xmlns:n="http://schemas.datacontract.org/2004/07/Microsoft.Robotics.Numerics"
  xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"
  xmlns:i="http://www.w3.org/2001/XMLSchema-instance"
  xmlns:w="http://schemas.datacontract.org/2004/07/Test.Robotics.Navigation.Metrics.PathPlanningAndTracking"
  exclude-result-prefixes="r k l m n a i">

  <xsl:include href="Common.xslt" />

  <xsl:template mode="default" match="text()[.='$map']">\\robofile\baker\Shared\Users\roboim\Maps\Racetrack\Racetrack.xml</xsl:template>

  <!-- Starting posing -->
  <xsl:template match="r:Agent[r:Name='StaticMapLocalization' or r:Name='SLAM2DAgent']/l:StartPose">
    <l:StartPose>
      <n:heading>1.57</n:heading>
      <n:x>600</n:x>
      <n:y>168</n:y>
    </l:StartPose>
  </xsl:template>

  <!-- Starting pose variance -->
  <xsl:template match="r:Agent[r:Name='StaticMapLocalization']/l:StartPoseVariance">
    <l:StartPoseVariance>
      <n:heading>2</n:heading>
      <n:x>200</n:x>
      <n:y>200</n:y>
    </l:StartPoseVariance>
  </xsl:template>

  <!-- Navigation waypoints (mock "store" space) -->
  <xsl:variable name="ne">
    <w:Waypoint>
      <w:IsDock>false</w:IsDock>
      <w:IsMarked>true</w:IsMarked>
      <w:LandmarkId>0</w:LandmarkId>
      <w:Pose>
        <n:heading>NaN</n:heading>
        <n:x>600</n:x>
        <n:y>168</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>0</n:heading>
        <n:x>0</n:x>
        <n:y>0</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>
  <xsl:variable name="nw">
    <w:Waypoint>
      <w:IsDock>false</w:IsDock>
      <w:IsMarked>false</w:IsMarked>
      <w:Pose>
        <n:heading>NaN</n:heading>
        <n:x>634</n:x>
        <n:y>327</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>NaN</n:heading>
        <n:x>NaN</n:x>
        <n:y>NaN</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>
  <xsl:variable name="center">
    <w:Waypoint>
      <w:IsDock>false</w:IsDock>
      <w:IsMarked>false</w:IsMarked>
      <w:Pose>
        <n:heading>NaN</n:heading>
        <n:x>510</n:x>
        <n:y>263</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>NaN</n:heading>
        <n:x>NaN</n:x>
        <n:y>NaN</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>
  <xsl:variable name="sw">
    <w:Waypoint>
      <w:IsDock>false</w:IsDock>
      <w:IsMarked>false</w:IsMarked>
      <w:Pose>
        <n:heading>NaN</n:heading>
        <n:x>403</n:x>
        <n:y>343</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>NaN</n:heading>
        <n:x>NaN</n:x>
        <n:y>NaN</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>
  <xsl:variable name="se">
    <w:Waypoint>
      <w:IsDock>false</w:IsDock>
      <w:IsMarked>false</w:IsMarked>
      <w:Pose>
        <n:heading>NaN</n:heading>
        <n:x>403</n:x>
        <n:y>180</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>NaN</n:heading>
        <n:x>NaN</n:x>
        <n:y>NaN</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>
  <xsl:variable name="dock">
    <w:Waypoint>
      <w:IsDock>true</w:IsDock>
      <w:IsMarked>false</w:IsMarked>
      <w:Pose>
        <n:heading>1.57</n:heading>
        <n:x>600</n:x>
        <n:y>200</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>NaN</n:heading>
        <n:x>NaN</n:x>
        <n:y>NaN</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>

  <!-- figure eight in the "store" course -->
  <xsl:template name="figure-eight">
    <xsl:copy-of select="$center" />
    <xsl:copy-of select="$nw" />
    <xsl:copy-of select="$ne" />
    <xsl:copy-of select="$center" />
    <xsl:copy-of select="$sw" />
    <xsl:copy-of select="$se" />
    <xsl:copy-of select="$dock" />
  </xsl:template>

  <!-- short docking run in the "store" course -->
  <xsl:template name="short-docking">
    <xsl:copy-of select="$center" />
    <xsl:copy-of select="$sw" />
    <xsl:copy-of select="$se" />
    <xsl:copy-of select="$dock" />
  </xsl:template>

  <!-- every point to every other in the "store" course -->
  <xsl:template name="long-circuit">
    <xsl:copy-of select="$se" />
    <xsl:copy-of select="$nw" />
    <xsl:copy-of select="$sw" />
    <xsl:copy-of select="$center" />
    <xsl:copy-of select="$se" />
    <xsl:copy-of select="$sw" />
    <xsl:copy-of select="$nw" />
    <xsl:copy-of select="$center" />
    <xsl:copy-of select="$ne" />
    <xsl:copy-of select="$nw" />
    <xsl:copy-of select="$se" />
    <xsl:copy-of select="$center" />
    <xsl:copy-of select="$sw" />
    <xsl:copy-of select="$ne" />
    <xsl:copy-of select="$sw" />
    <xsl:copy-of select="$se" />
    <xsl:copy-of select="$ne" />
    <xsl:copy-of select="$center" />
    <xsl:copy-of select="$nw" />
    <xsl:copy-of select="$dock" />
  </xsl:template>

  <!-- Metrics targets -->
  <xsl:template match="r:Agent[r:Name='PathPlanningAndTrackingMetricsControllerAgent']/m:Waypoints">
    <m:Waypoints>
      <xsl:call-template name="long-circuit" />
    </m:Waypoints>
  </xsl:template>

</xsl:stylesheet>
